
#include <std_msgs/Float64.h>
#include "ros/ros.h"
typedef union
{
    char buf[8];
    double data;
}TransDataFloat64;

namespace buu_ctrller
{
  class Ctrller
  {
    public:
        void callbackVehStat(const std_msgs::String::ConstPtr & msg)
        {
 //ROS_INFO("I heard: [%s]", msg);
            TransDataFloat64 veh_stat[4];
            for(int i=0;i<4;i++)
            {
                for(int j=0;j<8;++j)
                {
                    veh_stat[i].buf[j] = msg->data[8*i+j];
                }
            }
	
            pos_x_   =veh_stat[0].data;
            pos_y_   =veh_stat[1].data;
            heading_ =veh_stat[2].data;
            speed_   =veh_stat[3].data;
//ROS_INFO("pos_x_:[%d] pos_y_:[%d] heading_:[%d]  speed_:[%d]", veh_stat[0].data,veh_stat[1].data,veh_stat[2].data,veh_stat[3].data);
            flag1    = true;
        }
        void callbackPath(const std_msgs::String::ConstPtr & msg)
        {
		 
            path_x_.clear();
            path_y_.clear();	
            int len = msg->data.size();
            for(int i=0,j=0;i<len;i+=16)
            {
                TransDataFloat64 temp;
                memcpy(&temp,&msg->data[i],8);
//ROS_INFO("I heard: [%d]", temp.data);
                path_x_.push_back(temp.data);
                memcpy(&temp,&msg->data[i+8],8);
                path_y_.push_back(temp.data);
            }
	    flag2 = true;
	}
    public:
        void ctrl(std_msgs::String & cmd)
        {
            if(!(flag1 && flag2))
            {
                return;
            }
            double pre_len_= 5;
            double y_err = 0;
            std::vector<double> np_x,np_y;
            double new_x,new_y;
            for(int i=0;i<path_x_.size();++i)
            {

                transPt(path_x_[i],path_y_[i],pos_x_,pos_y_,heading_,new_x,new_y);
                if(new_x>pre_len_ )
                    break;
                np_x.push_back(new_x);
                np_y.push_back(new_y);

            }

            double opt_cuv = 2*new_y / (pre_len_*pre_len_);
            double opt_fw = opt_cuv * 2;
            TransDataFloat64 sw,speed;
            sw.data = opt_fw;//前轮转角
            speed.data = 3;//速度
            //发给小车的控制接口
            for (int i=0;i<8;++i)
            {
                cmd.data.push_back(sw.buf[i]);
            }
            for (int i=0;i<8;++i)
            {
                cmd.data.push_back(speed.buf[i]);
            }
            flag1 = false;
            flag2 = false;
        }

    private:
        double pos_x_,pos_y_,heading_,speed_;
        std::vector<double>path_x_,path_y_;

        bool flag1,flag2;
    private:
        void transPt(const double & old_x,
                     const double & old_y,
                     const double & new_in_old_x,
                     const double & new_in_old_y,
                     const double & new_from_old_theta,
                     double & new_x,double & new_y)
        {

            new_x = (old_x - new_in_old_x) * cos(new_from_old_theta) + (old_y - new_in_old_y) * sin(new_from_old_theta);
            new_y = (old_y - new_in_old_y) * cos(new_from_old_theta) - (old_x - new_in_old_x) * sin(new_from_old_theta);


            flag2 = true;
        }

  };
}

